import serial
import time
import threading

# 常量定义
MATH_PI = 3.1415926535897932

class AtCar:
    """
    类AtCar的定义
    Init函数参数:
        串口号: serial_number
        通信波特率: baud_rate
    """
    def __init__(self, serial_number, baud_rate=115200):
        self.serial_number = serial_number  # 串口号
        self.baud_rate = baud_rate  # 通信波特率
        self.ser_car = serial.Serial(serial_number, baud_rate, timeout=1)  # 打开串口，并设置读取超时时间为1秒
        # 小车参数
        self.wheel_radius = 0.04  # 轮子半径
        self.wheel_y_distance = 0.195  # 轮子左右间距
        self.wheel_x_distance = 0.13  # 轮子前后间距
        # 电机参数
        # 电机编号映射表
        # self.motor_dict = {'left_front': 7, 'right_front': 6, 'left_rear': 9, 'right_rear': 8}
        self.motor_nums = [7, 6, 9, 8]
        # 各个电机每转一圈脉冲数，单位为脉冲数，分别对应 左前、右前、左后、右后 电机
        self.motor_pulse_per_round = [749, 376, 377, 427]
        self.motor_pwms = [1500, 1500, 1500, 1500]  # 电机当前PWM值
        self.motor_speed = [0.0, 0.0, 0.0, 0.0]  # 电机当前速度
        self.motor_direction = [0, 0, 0, 0]  # 电机当前方向
        self.running = 1  # 线程运行标志, 1为运行，0为停止,-1为强制停止线程

        # 创建线程
        self.thread_car = threading.Thread(target=self.get_wheels_speed_and_set_motor_speed)
        # 如果串口已经连接，则启动线程
        self.init_serial_connection()

    def init_serial_connection(self):
        try:
            while True:
                ret = self.ser_car.read_all().decode()
                if '[FinishInit]' in ret:
                    break
            print("初始化完成")
            time.sleep(0.1)
            # print("串口已连接，启动线程")
            self.thread_car.start()
        except serial.SerialException as e:
            print(f"串口连接初始化失败: {e}")
            self.ser_car.close()
        except Exception as e:
            print(f"发生异常: {e}")

    def __del__(self):
        """
        析构函数，在对象被删除时调用
        """
        self.close()

    def close(self):
        """
        结束线程并关闭串口
        """
        self.running = -1
        time.sleep(0.1)  # 等待线程结束
        try:
            if self.thread_car.is_alive():
                self.thread_car.join(timeout=0.5)  # 等待线程结束，最多等待0.5秒
            self.ser_car.close()  # 关闭串口
        except Exception as e:
            print(f"关闭串口时发生异常: {e}")

    def get_raw_speed(self):
        """
        获取轮子速度
        """
        return self.motor_speed

    def set_motor_pwm(self, motor_pwms):
        self.motor_pwms = motor_pwms

    def get_wheels_speed_and_set_motor_speed(self):
        """
        获取轮子速度，并设置电机速度
        """
        running_time_set = 0.1

        while self.running == 1:
            cur_time = time.time()  # 获取当前时间，用于计算运行时间
            spd_set_cmd = "{{#{:03d}P{:04d}T1000!#{:03d}P{:04d}T1000!#{:03d}P{:04d}T1000!#{:03d}P{:04d}T1000!}}".format(
                self.motor_nums[0], self.motor_pwms[0], self.motor_nums[1], self.motor_pwms[1], self.motor_nums[2], self.motor_pwms[2], self.motor_nums[3], self.motor_pwms[3])  # 生成设置电机速度的命令
            try:
                self.ser_car.write(spd_set_cmd.encode())  # 发送设置电机速度的命令
            except serial.SerialException as e:
                print(f"发送设置电机速度命令时发生错误: {e}")
                self.running = -1
                break

            time.sleep(0.015)  # 等待15ms，以便接收到返回值
            ret = self.ser_car.read_all().decode()  # 接收返回值

            # 对于每一个电机，获取脉冲数
            for num in range(4):
                motor_number = self.motor_nums[num]  # 获取电机编号
                get_speed_cmd = "#{:03d}PSPD!".format(motor_number)  # 生成获取电机速度的命令
                try:
                    self.ser_car.write(get_speed_cmd.encode())  # 发送获取电机速度的命令
                except serial.SerialException as e:
                    print(f"发送获取电机{motor_number}速度命令时发生错误: {e}")
                    self.running = -1
                    break

                time.sleep(0.015)  # 等待15ms，以便接收到返回值
                ret = self.ser_car.read_all().decode()  # 接收返回值
                try:
                    ret_int = int(ret.split('=')[1].split('/')[0])  # 提取脉冲数
                except (IndexError, ValueError) as e:
                    print(f"解析电机{motor_number}速度时发生错误: {e}")
                    self.running = -1
                    break

                running_time = time.time() - cur_time  # 计算运行时间
                try:
                    self.motor_speed[num] = (ret_int / self.motor_pulse_per_round[num]) * 2 * MATH_PI * self.wheel_radius / running_time  # 计算电机速度，单位为m/s
                except ZeroDivisionError as e:
                    print(f"计算电机{motor_number}速度时发生错误: {e}")
                    self.running = -1
                    break

            running_time = time.time() - cur_time  # 计算运行时间
            if running_time < running_time_set:  # 如果运行时间小于设置时间，则sleep一会儿
                print("[NO]", running_time)
                time.sleep(running_time_set - running_time)
            else:
                print("[OVER]", running_time)

        if self.running == -1:
            print("线程被强制停止")


if __name__ == '__main__':
    # 创建AtCar对象
    atcar=AtCar("COM3")

    # 主程序

    import tkinter as tk

    # motor_pwm=[1500, 1500, 1500, 1500]

    def update_motor_pwm(event):
        global motor_pwm
        # 获取滑块的值并更新motor_pwm
        motor_pwm = [scale1.get(), scale2.get(), scale3.get(), scale4.get()]
        atcar.set_motor_pwm(motor_pwm)
        # print(f"Motor PWM Values: {motor_pwm}")

    # 创建主窗口
    root = tk.Tk()
    root.title("Motor PWM Control")

    # 创建四个滑块
    scale1 = tk.Scale(root, from_=500, to=2500, orient=tk.HORIZONTAL, label="Motor 1 PWM")
    scale1.set(1500)  # 设置初始值
    scale1.pack(fill=tk.X, padx=20, pady=5)

    scale2 = tk.Scale(root, from_=500, to=2500, orient=tk.HORIZONTAL, label="Motor 2 PWM")
    scale2.set(1500)  # 设置初始值
    scale2.pack(fill=tk.X, padx=20, pady=5)

    scale3 = tk.Scale(root, from_=500, to=2500, orient=tk.HORIZONTAL, label="Motor 3 PWM")
    scale3.set(1500)  # 设置初始值
    scale3.pack(fill=tk.X, padx=20, pady=5)

    scale4 = tk.Scale(root, from_=500, to=2500, orient=tk.HORIZONTAL, label="Motor 4 PWM")
    scale4.set(1500)  # 设置初始值
    scale4.pack(fill=tk.X, padx=20, pady=5)

    # 创建一个标签，用于显示当前的motor_pulses_per_revolutions值
    label = tk.Label(root, text="[0,0,0,0]")
    label.pack(pady=10)
    # 每隔100ms更新一次label显示的值
    def update_label():
        # 获取当前的motor_pulses_per_revolutions值
        motor_pulses_per_revolutions = atcar.get_raw_speed()
        label.config(text=f"{motor_pulses_per_revolutions}")

    # 创建一个按钮，用于更新motor_pwm值
    update_button = tk.Button(root, text="Update PWM", command=update_label)
    update_button.pack(pady=10)

    # 绑定事件处理函数:当滑块的值发生变化时，调用update_motor_pwm函数
    scale1.bind("<B1-Motion>", update_motor_pwm)
    scale2.bind("<B1-Motion>", update_motor_pwm)
    scale3.bind("<B1-Motion>", update_motor_pwm)
    scale4.bind("<B1-Motion>", update_motor_pwm)



    # 运行主循环
    root.mainloop()



